#include "cSTOCStereo.h"
#include "cwxOpenCV.h"

using namespace cv;

cSTOCStereo::cSTOCStereo(wxRadioBox* algorithmRadioBox)
{
	this->algorithmRadioBox = algorithmRadioBox;

	svsOpencv = new cSvsOpencv(320, 240, false);
	leftView = new cViewWindow(NULL, wxID_ANY, wxT("Left View"));
	rightView = new cViewWindow(NULL, wxID_ANY, wxT("Right View"));
	stereoView = new cViewWindow(NULL, wxID_ANY, wxT("Stereo View"));


	if(!svsOpencv->Open())
		wxMessageBox(wxT("Erro no Open"), wxT("ERRO"));

	started = false;
	fSaveVideo = false;
	fSaveScreenshot = false;
	fCalculateStereo = false;
	
	writer = NULL; writerR = NULL; writerS = NULL;

	Connect(wxID_ANY, wxEVT_COMMAND_MENU_SELECTED, wxCommandEventHandler(cSTOCStereo::OnThreadUpdate), NULL, this);

}

void cSTOCStereo::startCam(int aResH, int aResV, bool aIsColor, wxString extrinFile, wxString intrinFile)
{
	svsOpencv->setColor(aIsColor);
	svsOpencv->setRes(aResH, aResV);
	svsOpencv->setExtrinsicsFile(extrinFile.mb_str(wxConvUTF8));
	svsOpencv->setIntrisicsFile(intrinFile.mb_str(wxConvUTF8));

	leftView->SetSize(wxSize(aResH, aResV));
	rightView->SetSize(wxSize(aResH, aResV));
	stereoView->SetSize(wxSize(aResH, aResV));

	leftView->Show();
	rightView->Show();

	if (Create() != wxTHREAD_NO_ERROR)
	{
		wxLogError(wxT("Could not create the worker thread!"));
		return;
	}


	svsOpencv->Start();

	if (GetThread()->Run() != wxTHREAD_NO_ERROR)
	{
		wxLogError(wxT("Could not run the worker thread!"));
		return;
	}

	started = true;
}

void cSTOCStereo::stopCam()
{
	if(started)
	{
		GetThread()->Delete();
		svsOpencv->Stop();
		leftView->Show(false);
		rightView->Show(false);
		started = false;
	}
}

void cSTOCStereo::startSaveVideo(wxString vName, bool sLeft, bool sRight, bool sStereo) //fix condition to start
{
	wxString rVName(vName);
	wxString sVName(vName);
	vName += wxT("L.avi");
	rVName += wxT("R.avi");
	sVName += wxT("S.avi");

	if(sLeft)
		writer = cvCreateVideoWriter(vName.mb_str(wxConvUTF8), CV_FOURCC('P', 'I', 'M', '1'),
		                             25, cvSize(imgStereo->width, imgStereo->height), 1);

	if(sRight)
		writerR = cvCreateVideoWriter(rVName.mb_str(wxConvUTF8), CV_FOURCC('P', 'I', 'M', '1'),
		                              25, cvSize(imgStereo->width, imgStereo->height), 1);

	if(sStereo)
		writerS = cvCreateVideoWriter(sVName.mb_str(wxConvUTF8), CV_FOURCC('P', 'I', 'M', '1'),
		                              25, cvSize(imgStereo->width, imgStereo->height), 1);

	fSaveVideo = true;
}

void cSTOCStereo::stopSaveVideo()  //fix for thread safe
{
	fSaveVideo = false;
	if(writer) cvReleaseVideoWriter(&writer);
	if(writerR) cvReleaseVideoWriter(&writerR);
	if(writerS) cvReleaseVideoWriter(&writerS);
}

void cSTOCStereo::saveScreenshot(wxString ssName)
{
	screenshotName = ssName;
	fSaveScreenshot = true;
}

void cSTOCStereo::startCalculateStereo(int numberD, int SADs)
{
	svsOpencv->setNumberOfDisparities(numberD);
	svsOpencv->setSADWindowSize(SADs);
	fCalculateStereo = true;
	stereoView->Show();
}
void cSTOCStereo::stopCalculateStereo()
{
	fCalculateStereo = false;
	stereoView->Show(false);
}

void* cSTOCStereo::Entry()
{

	while (!GetThread()->TestDestroy())
	{

		dataCS.Enter();
		svsOpencv->refresh();
		imgLeft = svsOpencv->getLeft();
		imgRight = svsOpencv->getRight();
		dataCS.Leave();

		if (fCalculateStereo)
		{
			svsOpencv->setStereoAlgo(algorithmRadioBox->GetSelection());
			dataCS.Enter();
			imgStereo = svsOpencv->calculateStereo();
			dataCS.Leave();
		}

		if (fSaveVideo)
		{
			if(writer) cvWriteFrame(writer, imgLeft);
			if(writerR) cvWriteFrame(writerR, imgRight);
			if(writerS) cvWriteFrame(writerS, imgStereo);
		}

		if (fSaveScreenshot)
		{
			wxString name;
			name = screenshotName;
			name += wxT("L.bmp");
			cvSaveImage(name.mb_str(wxConvUTF8), imgLeft);
			name.Clear();
			name = screenshotName;
			name += wxT("R.bmp");
			cvSaveImage(name.mb_str(wxConvUTF8), imgRight);
			fSaveScreenshot = false;
		}

		AddPendingEvent(*new wxCommandEvent(wxEVT_COMMAND_MENU_SELECTED));
		wxMilliSleep(35);

	}
}


cSTOCStereo::~cSTOCStereo()
{
	stopCam();
	leftView->Destroy();
	rightView->Destroy();
	stereoView->Destroy();
	delete svsOpencv;
}

void cSTOCStereo::OnThreadUpdate(wxCommandEvent& evt)
{
	wxClientDC dc(leftView->getPanel()), dcR(rightView->getPanel()), dcD(stereoView->getPanel());

	if(!imgLeft)
		return;

	dataCS.Enter();

	cwxOpenCV::convertIplToBMP(imgLeft, outLeft);
	cwxOpenCV::convertIplToBMP(imgRight, outRight);

	dataCS.Leave();

	dc.DrawBitmap(outLeft, 0, 0, 0);
	dcR.DrawBitmap(outRight, 0, 0, 0);

	if(fCalculateStereo)
	{
		dataCS.Enter();
		cwxOpenCV::convertIplToBMP(imgStereo, dispBMP);
		dataCS.Leave();
		dcD.DrawBitmap(dispBMP, 0, 0, 0);
	}

}
